/*
 *  PogoStick.m
 *  pogostick
 *
 *  Created by daddy (chuck@kollar.com) on 8/23/09.
 *  Copyright 2009 Charles P. Kollar All rights reserved.
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */

#import "PogoStick.h"


@implementation PogoStick

@synthesize space;
@synthesize stickHeight;
@synthesize springLength;
@synthesize footRadius;
@synthesize footTouchedFloorCount;
@synthesize stickMass;
@synthesize currentGravity;
@synthesize group;
@synthesize stickBody;
@synthesize handHoldL;
@synthesize handHoldR;
@synthesize footBody;
@synthesize footJointStaticBody;
@synthesize stickJointStaticBody;


// set in menu.m by the user from yes/no buttons...
extern int soundp, musicp;

// This is set by the slider callback 'springSliderAction' found in GameLayer.m
// This parameter is set by the user....
extern float stickSpringPower;


/**
 The pogostick has a body and a plunger (foot). The body is attached to a static body (sb) by a gear joint
 so that it's rotation can be controlled. The plunger (foot) is alas attached to the static body by a gear
 joint. The plunger is attached to the body by one groove joints. The idea was to try to constrain the plunger
 to move along the axis of the body using the joints. A small force is also placed on the plunger to
 make it generally try to extend. When the circle (ball) on the foot strikes the ground, an impluse is give
 to the body at a vector which is at the angle of the body. You work the app by tilting the (iPod) ground in
 the application while the pogostick always stays verticle w.r.t. the ground under your feet. In this manner
 it will strike the ground at what ever angle you hold the iPod, and go bouncing off in that direction.
 http://www.slembcke.net/forums/viewtopic.php?t=851
*/
-(id)initInSpace: (cpSpace *)sp toStaticBody: (cpBody *)sb gravity: (cpFloat)gra group: (unsigned int)gro position: (cpVect)p size: (cpVect)size;
{
    if (self = [super init])
    {
        cpConstraint *joint;
        
        [self setFootTouchedFloorCount: 0];
		
		[self setSpace: sp];
		[self setStickHeight: size.y];
		[self setSpringLength: size.y*0.7f];
		[self setStickMass: 15.0f];
		[self setCurrentGravity: gra];
		[self setGroup: gro];
        [self setFootRadius: size.x*.4f];
        
        cpFloat armThickness = footRadius / 8.0f;
		cpFloat footMass = stickMass / 20.0f;
		
		cpVect verts[] = {
			cpv(-footRadius*.75f,-stickHeight),	// bottom left
			cpv(-footRadius*.75f, stickHeight),	// top left
			cpv( footRadius*.75f, stickHeight),	// top right
			cpv( footRadius*.75f,-stickHeight)	// lower right
		};
		stickBody = cpBodyNew(stickMass, cpMomentForPoly(stickMass, (sizeof(verts)/sizeof(cpVect)), verts, cpvzero));
		stickBody->p = p;
		cpSpaceAddBody(space, stickBody);
        
        // This is the main "stick" shape of the pogostick
		cpShape *shape = cpPolyShapeNew(stickBody, (sizeof(verts)/sizeof(cpVect)), verts, cpvzero);
		shape->e = 0.0f; shape->u = 1.0f;
		shape->group = group;
        shape->collision_type = kColl_Stick;
		cpSpaceAddShape(space, shape);

		/*  http://www.slembcke.net/forums/viewtopic.php?t=709
		 You would just need to attach a gear joint with a ratio of 1:1 from your plank's body 
		 to whatever static body your world is using. Using the phase parameter of the joint,
		 you can control the angle it goes towards and then use the max force, max bias, and
		 bias coefficient to control of fast and strongly the plank rights itself.
		 phase of M_PI/2 is 180; 0.0f is 90 (vertical) */
		stickJointStaticBody = cpGearJointNew(sb, stickBody, 0.0f, 1.0f);
		/*  maxForce: In the case of rotary joints it's actually the maximum torque.
		 This really depends on the mass of the objects and more importantly their size.
		 I almost always just guess by starting with 10 then increasing by 10x until it's
		 about right and then adjust until it looks right.	*/
		stickJointStaticBody->maxForce = 1e7; // something less than INFINITY that lets the stick wabble a bit...
		/*  biasCoef: This is the fraction of the joint error (how far the joint is separated) that
		 should be resolved in each step. The default is 0.1. Larger values make the joints seem
		 more solid, but can make the simulation less stable. Valid values are 0 to 1 not including 0. */
		//jointStaticBody->biasCoef;
		/* maxBias: This is actually referring to the max speed the joint is corrected at.
		 In the case of rotary joints, the units are radians per second. The default is infinity,
		 so the rate of correction applied will increases as the amount of joint error does.
		 The bias stuff might sound a bit abstract, but try playing with the values.
		 You'll probably not need to adjust the biasCoef, but it's helpful to know what it does. */
		stickJointStaticBody->maxBias = 5.0f; // default is inf, but bias rarely goes over 10.0f
		cpSpaceAddConstraint(space, stickJointStaticBody);
        
        // Here we create the handles and foot things on the top and bottom of the shape above
        // to make something that passes as a pogostick...
		cpVect armverts[] = {
			cpv(-footRadius*2,-armThickness),
			cpv(-footRadius*2, armThickness),
			cpv( footRadius*2, armThickness),
			cpv( footRadius*2,-armThickness)
		};
		
		shape = cpPolyShapeNew(stickBody, (sizeof(armverts)/sizeof(cpVect)), armverts, cpv(0.0f,stickHeight-(stickHeight/10.0f)));
		shape->e = 0.0f; shape->u = 1.0f;
		shape->group = group;
        shape->collision_type = kColl_Stick;
		cpSpaceAddShape(space, shape);
		
		shape = cpPolyShapeNew(stickBody, (sizeof(armverts)/sizeof(cpVect)), armverts, cpv(0.0f,-stickHeight+armThickness*2));
		shape->e = 0.0f; shape->u = 1.0f;
		shape->group = group;
        shape->collision_type = kColl_Stick;
		cpSpaceAddShape(space, shape);
	
        cpFloat relativePlungerLength = 0.75f;
        // Here we make the body asociated with the foot. The body is the framework on which the
        // shapes are created. Notice that the body for the foot/plunger is created as relative
        // to the stick body, as are the shapes associated with the plunger....
		footBody = cpBodyNew(footMass, cpMomentForCircle(footMass, 0.0f, footRadius, cpvzero));
        footBody->p = cpvadd(stickBody->p, cpv(0.0f, -stickHeight*(1.0f - relativePlungerLength)));
		footBody->v = stickBody->v;
		footBody->data = stickBody; // so we can get it when the foot strikes the ground
		cpSpaceAddBody(space, footBody);
        
        // Use the same gear joint trick to attach the foot to the static body as well.
        // This will rquire both joits to be turned by the "orientation" method below...
        footJointStaticBody = cpGearJointNew(sb, footBody, 0.0f, 1.0f);
		footJointStaticBody->maxForce = 1e7; // something less than INFINITY that lets the stick wabble a bit...
		footJointStaticBody->maxBias = 5.0f; // default is inf, but bias rarely goes over 10.0f
		cpSpaceAddConstraint(space, footJointStaticBody);
        
		// create the plunger...
		cpVect verts2[] = {
			cpv(-footRadius*.65f,-stickHeight*relativePlungerLength),
			cpv(-footRadius*.65f, stickHeight*relativePlungerLength),
			cpv( footRadius*.65f, stickHeight*relativePlungerLength),
			cpv( footRadius*.65f,-stickHeight*relativePlungerLength)
		};
        
		shape = cpPolyShapeNew(footBody, (sizeof(verts2)/sizeof(cpVect)), verts2, cpvzero);
		shape->e = 0.0f; shape->u = 1.0f;
		shape->group = group;
        shape->collision_type = kColl_Stick;
		cpSpaceAddShape(space, shape);
        
        // Create the circle just below the plunger which serves as the foot that strikes
        // against the ground which causes the pogostick to jump.
		shape = cpCircleShapeNew(footBody, footRadius, cpv(0.0f, -stickHeight));
		// we want the foot to grip well with a small squish factor...
		shape->e = 1.5f; shape->u = 50.0f;
        shape->group = group;
        shape->collision_type = kColl_Foot;
		cpSpaceAddShape(space, shape);

		// The plunger (pollyshape of the footBody) is attached to the body with one groove joint.
		// The groove is on the stickBody and goes from the first point to the second point.
		// The pivot is attached to the footBody at the third point.
        // It doesn't matter so much exactly where the attachment is made.
        // Remember that both the stick and the foot are already constrained to be at the same
        // orientation by the gear joints.

        // Here a grove is in the stickHeight bottom of the stickBody attached to the center of the plunger...
		joint = cpGrooveJointNew(stickBody, footBody, cpv(0.0f,-stickHeight), cpv(0.0f,-stickHeight*(1.0f - relativePlungerLength)), cpvzero);
        joint->maxForce = INFINITY;
		cpSpaceAddConstraint(space, joint);
#if 0        
        // DEBUG CODE: put circles where I think that the groove joint attachments should be so that I can see them.
        shape = cpCircleShapeNew(stickBody, 4.0f, cpv(0.0f,-stickHeight));
        shape->group = group;
		cpSpaceAddShape(space, shape);
        shape = cpCircleShapeNew(stickBody, 4.0f, cpv(0.0f,-stickHeight*(1.0f - relativePlungerLength)));
        shape->group = group;
		cpSpaceAddShape(space, shape);
        shape = cpCircleShapeNew(footBody, 2.0f, cpvzero);
        shape->group = group;
		cpSpaceAddShape(space, shape);
#endif
        // a small force "not an impulse" is kept on the plunger to make it tend to extend...
        [self extendPlunger];
        
		handHoldR = cpv(15.0f, stickHeight-armThickness*2);
		handHoldL = cpv(-15.0f, stickHeight-armThickness*2);
	}
	return self;
}

/**
 * This method is used by the accelerometer defined in GameLayer.m to change the orientation 
 * of the pogostick. We must modify both gear joints here.
 */
-(void)orientation: (cpFloat)phase {
    cpGearJoint *j = (cpGearJoint *)footJointStaticBody;
    j->phase = phase;
    j = (cpGearJoint *)stickJointStaticBody;
    j->phase = phase;
}

#define PLUNGER_EXTEND_GRAVITY_MULTIPLIER       8.0f

/**
 * Puts a force on the plunger to extend it.
 */
-(void)extendPlunger {
    cpVect angle = cpvforangle(0.0f);
    [self extendPlunger: angle];
}
-(void)extendPlunger: (cpVect)angle {
    // cause the "spring" to extend...
    cpVect footForce = cpvmult(angle, currentGravity*PLUNGER_EXTEND_GRAVITY_MULTIPLIER);
    cpBodyApplyForce(footBody, footForce, cpvzero);
}

/**
 * This method is called to move the pogostick to a new position.
 * In this game there is really one reason to do this, and that is that the
 * pogostick has gone below the lowest floor level (e.g. has died). Siff...
 * This method is only called in the performActions method of Level.
 */
-(void) moveToPosition: (cpVect)p {
    stickBody->p = p;
    footBody->p = cpvadd(p, cpv(0.0f, -stickHeight-footRadius));
    
    stickBody->v = cpvzero;
    footBody->v = cpvzero;

    cpBodySetAngle(stickBody, 0.0f);
	cpBodySetAngle(footBody, 0.0f);

    cpBodyResetForces(stickBody);
	cpBodyResetForces(footBody);
    
    // Since we reset the forces on the foot (plunger)...
    [self extendPlunger];
}

-(void) jump {
    // stop all movement on the stick....
    cpBodyResetForces(stickBody);
    stickBody->v = cpvzero; stickBody->w = 0.0f;
    
    // A short pulse of energy to simulate the spring. Not additive.
    // The impulse is applied at the center of gravity in the direction that the stick
    // was leaning when it touched the ground. The "lean" of the stick is controlled
    // by tilting the iPod left or right.
    cpFloat magnitudeF = -currentGravity * stickSpringPower;
    cpVect angle = cpvforangle(stickBody->a + M_PI/2); // 180 degrees out of phase (e.g, the opposite direction)
    cpVect impulse = cpvmult(angle, magnitudeF);
    //NSLog(@"makeStickJump angle %f  a(%f,%f)  impulse(%f,%f)\n", stickBody->a, angle.x, angle.y, impulse.x, impulse.y);
    cpBodyApplyImpulse(stickBody, impulse, cpvzero);
    
    // Whether there is sound or not is set by the user in the configuration menu.
    if (soundp != 0) {
        [[SimpleAudioEngine sharedEngine] playEffect:STICK_JUMP_WAV];
    }      
    
    // We need to do this since the "foot hit ground" collision code will stop all movenent
    // on the stick and the foot.
    [self extendPlunger: angle];
}

-(void) dealloc {
	[super dealloc];
}

@end
